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METHOD AND SYSTEM FOR GUIDING A VEHICLE 

INTRODUCTION 

5 

Field of the Invention 

The invention relates generally to control of vehicle movement in confined spaces 
where a human operator has a limited view of the surroundings. The vehicle may, 
10 for example, be an automobile, a boat, a wheelchair for a visually impaired person, a 
rollator for a visually impaired person, or a submersible. 

Prior Art Discussion 

15 United States Patent No. US6172641 (Magellan) describes an in-vehicle navigation 
system which generates audible instructions for a driver to guide the driver on a 
route. The system uses a stored map and a current vehicle location to generate the 
instructions. 

20 While such a system appears to be effective for some situations, the extent of 
information provided to the driver is limited. 

The invention is therefore directed towards providing for more comprehensive 
operator guidance for manoeuvring vehicles. 

25 

SUMMARY OF THE INVENTION 



30 



According to the invention, there is provided a vehicle guidance system comprising 
means for detecting position of a vehicle and a processor for generating operator 
instructions to assist the operator to manoeuvre the vehicle, characterised in that 
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the system comprises range sensors and means for using outputs of the 
sensors to dynamically maintain a space map in real time; 

5 the system comprises an operator model of operator capabilities; and 

the processor comprises means for planning a path to a goal position in a 
confined space using data from the operator model of operator capabilities 
and data from the space map. 

10 

Use of range sensors allows space around the vehicle to be dynamically monitored 
for guidance. The fact that the operator characteristics are also used for guidance 
ensures that the operator is in control irrespective of his or her capabilities. Thus, the 
invention achieves the benefit of automated sensing for path planning and collision 
15 avoidance, while leaving the operator in control. 

In one embodiment, the system comprises means for maintaining the space map as a 
set of polygons each defining a spatial area. 

20 In another embodiment, said space map maintaining means comprises means for 
processing three-dimensional space data from the sensors and for breaking said data 
to two-dimensional space data in a polygon framework. 

In a further embodiment, said space map maintaining means comprises means for 
25 maintaining a flag for each polygon, said flag indicating if the polygon is occupied by 
an object. 

In one embodiment, the operator model comprises time parameter values for the 
operator to perceive information, for the operator to cognitively process information, 
30 and for neuromuscular delay for implementing an action. 
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In another embodiment, die operator model comprises a parameter value for spatial 
error for performing an action. 

5 In a further embodiment, the processor comprises means for storing a vehicle model 
of movement characteristics of the vehicle. 

In one embodiment, the processor comprises path planning means for planning a 
plurality of sub-goals forming a path to the goal position. 

10 

In another embodiment, the path planning means comprises means for determining a 
sub-goal only if it can be achieved with a single manoeuvre of the vehicle and if it is 
instructable to the operator. 

15 In a further embodiment, the path planning means comprises means for determining 
a set of candidate trajectories for reaching the goal position with collision avoidance, 
and for performing backchaining to break each candidate trajectory into a plurality of 
sub-goals starting from the goal position and extending back to the current position. 

20 In one embodiment, the set of candidate trajectories is determined by generating a 
first set of candidate trajectories compatible with the space and the vehicle models, 
and for subsequently reducing the first set to those compatible with the operator 
model. 

25 In another embodiment, the path planning means comprises means for dynamically 
repeating path planning in real time at each of a plurality of vehicle positions. 

In a further embodiment, the vehicle model is for a boat, and the sensors comprise a 
sonar device, a laser range finder, a vision system, a radar system, and a GPS circuit. 

30 
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In another aspect, the invention provides a vehicle comprising a guidance system as 
defined above. 

In a further aspect, the invention provides a method of guiding a vehicle from a 
5 current position to a goal position in a confined space, the method comprising the 
steps of:- 

range sensors automatically sensing objects around the vehicle, 

10 a processor using outputs of the range sensors to dynamically maintain a 

space map in real time, 

the processor planning a path to the goal position using data from the space 
map and from an operator model of operator capabilities, 

15 

an output interface outputting instructions to the operator for movement of 
the vehicle according to the planned path, and 

the operator moving the vehicle according to the instructions. 

20 

In one embodiment, the processor generates instructions for the operator for 
movement of the vehicle in a single manoeuvre at a time. 

In another embodiment, the processor performs backchaining to break down a path 
25 into a plurality of sub-goals, and generates an operator instruction to achieve each 
sub-goal in turn. 

In a further embodiment, the processor repeats path planning at each position on the 
basis of the current position. 

30 
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In one embodiment, the processor generates the operator instructions also using data 
from a vehicle model of vehicle movement characteristics. 

DETAILED DESCRIPTION OF THE INVENTION 

5 

Brief Description of die Drawings 

The invention will be more clearly understood from the following description of 
some embodiments thereof, given by way of example only with reference to the 
10 accompanying drawings in which: - 

Fig. 1 is a diagram illustrating a guidance system for a boat; 

. Fig. 2 is a flow diagram illustrating overall operation of the guidance system; 

15 

Figs. 3(a) to 3(f) are diagrams illustrating movement of the boat; 

Fig. 4 is a flow diagram illustrating how the system determines a path to 
achieve a goal; 

20 

Fig. 5 is a perspective view of those parts of the guidance which interact with 
an operator; 

Figs. 6(a) to 6(f) are a set of illustrations corresponding to Figs. 3(a) to 3(f); 

25 

Fig. 7 is a diagram illustrating alternative path planning for a boat; and 

Fig. 8 is a perspective view of a walking frame incorporating a guidance 
system. 

3a 
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Description of the Embodiments 

Referring to Fig. 1 a guidance system 1 for a boat is illustrated. The system 1 is 
particularly for assisting a pilot to manoeuvre a large boat into a dock from a position 
5 either inside or outside a harbour. In many instances the available space for docking 
large boats is very confined. 

The system 1 comprises a processor 2 and range sensors for sensing objects in the 
space surrounding the boat. These sensors are sonar devices 3, laser range finders 4, 

10 vision systems 5, radar systems 6 and GPS 7. A stored vehicle model 10 and a stored 
operator model 11 are accessible to the processor 2. These models are updated 
regularly, but are not truly dynamic in the sense of being updated in real time during 
docking. The system 1 also comprises a random access memory (RAM) dynamic 
space map 12 which is dynamically updated in real time. There is also a general 

15 RAM 15, and a stored spatial map 8. 

Finally, the system 1 comprises various output devices for guiding an operator, 
namely a liquid crystal display (LCD) 16, a speech synthesiser 17, warning devices 
18 for sounding audible warnings and having flashing beacons, and a haptic feedback 
20 mechanism 19 in the helm. Some of these devices are also shown more illustratively 
in Fig. 5. 

Referring to Fig. 2 operation of the system 1 in a method 21 to reach a goal of 
docking the boat is illustrated. In step 22 the operator may request assistance by 
25 inputting data indicating location of the dock according to coordinates of the space 
map 12. If a goal has not been provided by step 22 a goal may be inferred by step 23. 
This goal is determined by analysing the space and previous manoeuvres to infer the 
likely goal. 
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The system 1 plans a path to the goal in step 24, illustrated in more detail in Fig. 4. 
The path comprises a series of sub-goals which are reached by the operator 
manoeuvring the boat with the assistance of prompts automatically generated by the 
system 1. The sub-goal movements may be forward, reverse, rotational, or curved 
5 moves. An important aspect of the system 1 is that the path is planned on the basis 
of: 

the dynamic space map 12 as well as the stored map 8, taking account of 
transient obstacles and conditions generally in real time, 

10 

breaking down the path into-sub-goal manoeuvres determined according to 
the capabilities of the human operator aswell as those of the boat; and 

breaking down the path into sub-goal manoeuvres determined according to 
15 what is instructable to the operator at any one point in time. 

A path is illustrated diagrammatically in Figs. 3(a) to 3(f) and in perspective in 
corresponding Figs. 6(a) to 6(f). In this diagram the boat is indicated by the numeral 
30 and the target dock by 31. The boat 30 is to move from a start position 32 to a 
20 goal position 33 in the dock 31. The start and goal positions 32 and 33 are shown in 
Fig. 3(a). A Sub-goal 1 (Fig. 3(b)) is a straight-ahead movement. A Sub-goal 2 (Fig. 
3(c)) involves another straight-ahead movement. A Sub-goal 4 (Fig. 3(e)) involves a 
rotational movement, and a Sub-goal 5 (Fig. 3(f)) involves a. final linear 
(translational) movement into position in the dock 31 at the goal position 33. 

25 

It will be appreciated that each of the movements is relatively simple and may be 
achieved by the operator under system instructions. 

Execution of the planned path as described above is initiated by outputting an 
30 instruction in step 25, and in step 26 the system determines if the sub-goal has been 
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reached. If the operator has not taken the required action to head toward the sub- 
goal another instruction is outputted. Upon reaching the sub-goal the system outputs 
an instruction for the next sub-goal as indicated by the decision step 27, until the goal 
is reached. 

5 

At any time the system issues a warning using the warning devices 18 if movement is 
deemed to be unsafe, such as likelihood of collision. This is detected by the sensors 
3-7. These steps are steps 29(a) and 29(b) of Fig. 2. 

10 Referring now to Fig. 4 the step 24 of planning the path is shown in more detail. 
During path planning and at all other times the system:- 

- processes sensor outputs (40) and updates (41) the space map 12, 

15 - captures (42) operator responses and updates (43) an operator model, and 

- captures (44) vehicle responses and updates (45) a vehicle map. 

As stated above the space map 12 is updated in real time, whereas updating of the 
20 vehicle and operator maps is near real-time. 

Space Map 

The space map comprises a set of polygons defining spatial areas in plan. Steps 40 
25 and 41 comprise dynamically using the sensor outputs in real time to monitor 
occupancy of volumes an a 3D basis and breaking the 3D data down to a 2D plan of 
polygons. These polygons are dynamically flagged as being either occupied or 
unoccupied. The space map 12 also generates a list of all occupied polygons. This 
information is used to both plan the path and to avoid collisions during execution. 
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Operator Model 

The operator model is defined using concepts set out in the paper "The Psychology 
of Human - Computer Interaction" by Card, Newell and Moran published by 
5 Lawrence Enbaum Associates in 1983. The parameters are time and distance. The 
time parameters are expressed in ms, and are: 

- time for the operator to perceive information, 

10 - time for the operator to cognitively process information to perform an action, 
and 

- time for the operator to implement the action, based on neuromuscular delay. 

15 The distance parameter is a spatial error in performing the action. This takes 
account of the fact that there is considerably more human error in spatial movement 
terms than for robots. The distance parameter may be in translational or rotational 
terms. 

20 Vehicle Model 

The vehicle model is defined using concepts from the physics of motion. Important 
model parameters are how steering angles from the helm affect movement of the 
boat, boat displacement at different engine output power levels, the present water 
25 line at the dock 3 1 , and wind and tide directions. 

Path Planning 



30 



The path planning approach is inspired by the notion of pre-image back chaining as 
described in "Automatic synthesis of fine-motion strategies for robots" by T Cozano- 
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Perez and M.A. Wesley, published in the International Journal of Robotic Research, 
3(1): 3-24, 1984. To make the benefits of automatic path planning available to a 
human operator the approach has been expanded significantly to allow for the 
limitations of the human operator and dynamic properties of the space map and 
5 vehicle dynamics. 

Referring again to Fig. 4, as well as the background steps 40 - 45, the system 
performs steps 46 to 51 to plan a path (overall step 24). 

10 In step 46 the system simulates movement of the boat to the goal position in a 
number of different trajectories. For each trajectory, the system determines if there 
would be a collision. The system uses the space and vehicle models to determine if 
collisions will occur when "pushing" the boat through to the goal position 33. The 
result of step 46 is that a first set of candidate trajectories is outputted in step 47. 

15 

The system then in step 48 uses the operator model 11 to limit the first candidate set 
to those which the operator is likely to be able to achieve, i.e. which are instructable. 

In step 49 the system 1 performs backchaining to break down each candidate 
20 trajectory of the second set in turn. This involves 

(a) starting at the goal position 33, 

(b) moving back to a position from which the boat can reach the goal position 33 in 
25 an instructable movement, and 



(c) repeating (a) and (b) for each successive sub-goal backwards to the current 
position. 
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Backchaining is performed for each candidate trajectory of the second set, as 
indicated by the decision step 50. In step 51 the system selects one of these 
trajectories as the planned path. 

5 Fig. 7 shows an example of a boat 57, moving along a trajectory (path) P. If the goal 
is to move the boat to the other side of sand banks 54 then two regions 52, 53 can be 
identified on the current trajectory from which this goal is achievable. The beginning 
and end of the sub-goal regions 52, 53 are identified by the regions 56, 57, 58, and 
59. These regions indicate the range of headings that the human operator is most 
10 likely to achieve if instructed to turn in the direction of the arrows. The range of 
headings used in the path planning depends on both the manoeuvrability of the boat 
and the skill of the human operator. The system will choose between the sub-goals 
52 and 53 on the basis of safety, total distance to the goal, speed to reach the goal, 
and minimising the number of sub-goals to reach the goal. 

15 

Thus, the output of the path planning step is a set of sub-goals, each reachable in a 
single boat manoeuvre and being individually instructable to the operator (pilot). 
The latter aspect is particularly important as it ensures that the operator (pilot) is at 
all times in control and manoeuvring of the boat is not autonomous. Furthermore, 
20 the path planning steps is dynamically repeated on an interactive basis as the boat 
moves. This is to take account of changes in conditions reflected in the space map in 
real time. 

It will be appreciated that the system 1 allows a vehicle to move in attainable 
25 movements tailored to the abilities of the operator and of the vehicle. Also, the 
system does not rely exclusively on pre-set maps or GPS location data, and instead 
uses sensor outputs in real time to dynamically maintain a more meaningful space 
map of vehicle location data and of possible obstructions. This takes account of 
unexpected movement of third parties or objects and ensures that at all times 
30 effective and safe movement is possible. Another important advantage is that the 
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operator is responsible for movement, the system providing comprehensive 
assistance but not taking over control. 

The invention is not limited to the embodiments described but may be varied in 
5 construction and detail. For example, the invention may be applied to other vehicles 
such as cars, lorries, wheelchairs, walking frames, or submersibles. The sensors 
would be chosen to suit the particular environment of the vehicle. Another 
embodiment is a guidance system for a visually impaired person using a rollator 60 
having a frame 61 and wheels 62 as shown in Fig. 8. The system 60 provides a user 
10 with the ability of manoeuvring their walking aid in confined spaces without the fear 
of colliding with people or objects around them. In this embodiment, the orientation 
system comprises sonar devices 63, a laser range finder 64, a haptic feedback steering 
handlebar 65, and a speech synthesiser 66. This guidance system operates on the 
same principles as the boat guidance system described above. 



